#include "pid_ctrl.h"
#include "log.h"

const static char *TAG = "pid_ctrl.c";

/**
 * @brief pid_init
 *
 * @param PID pid struct
 * @param set_point target value
 * @param actualvalue expectations
 * @param Kp
 * @param Ki
 * @param Kd
 */
void pid_init(PID_TypeDef *PID, float set_point, float actualvalue, float Kp, float Ki, float Kd)
{
    if (PID == NULL)
    {
        MY_LOGI(TAG, "Error: PID pointer is NULL\r\n");
    }
    else
    {
        PID->set_point = set_point;
        PID->actualvalue = actualvalue;
        PID->Kp = Kp;
        PID->Ki = Ki;
        PID->Kd = Kd;
        MY_LOGI(TAG, "pid init success!\r\n");
    }
}

/**
 * @brief pid_position_ctrl (uk = Kp*e_k + Ki*Σe_j + Kd*(e_k - e_k-1))
 *
 * @param PID
 * @param input
 * @return float
 */
float pid_location_style_ctrl(PID_TypeDef *PID, float input, float integral_limit)
{
    PID->error = PID->set_point - input;
    PID->sum_error += PID->error;
    /* Add input Limiting processing begin */
    if (PID->sum_error >= integral_limit) /* Avoid integral saturation */
    {
        PID->sum_error = integral_limit;
    }
    else if (PID->sum_error <= -integral_limit)
    {
        PID->sum_error = -integral_limit;
    }
    /* Add input Limiting processing end */
    PID->actualvalue = (PID->Kp * PID->error)                        /* Kp*e_k */
                       + (PID->Ki * PID->sum_error)                  /* Ki*Σe_j */
                       + (PID->Kd * (PID->error - PID->last_error)); /* Kd*(e_k - e_k-1) */
    PID->last_error = PID->error;                                    /* update error */

    /* Add output Limiting processing begin */

    /* Add output Limiting processing end */
    return PID->actualvalue;
}

/**
 * @brief pid_incremental_ctrl (uk = Kp*(e_k - e_j) + Ki*e_k + Kd*(e_k - 2e_k-1 +e_k-2))
 *
 * @param PID
 * @param input
 * @return float
 */
float pid_incremental_ctrl(PID_TypeDef *PID, float input)
{
    PID->error = (PID->set_point - input);
    PID->actualvalue += (PID->Kp * (PID->error - PID->last_error))                          /* Kp*(e_k - e_j) */
                        + (PID->Ki * PID->error)                                            /* Ki*e_k */
                        + (PID->Kd * (PID->error - 2 * PID->last_error + PID->prev_error)); /* Kd*(e_k - 2e_k-1 +e_k-2) */
    PID->prev_error = PID->last_error;                                                      /* update n-2 error */
    PID->last_error = PID->error;                                                           /* update n-1 error */

    /* Add output Limiting processing begin */

    /* Add output Limiting processing end */
    return PID->actualvalue;
}
